#pragma once

#include <osg/Matrix>
#include <osg/Vec3>
#include <osg/Vec4>
#include "corebase_global.h"

namespace CoreBase
{
	template <typename Real>
	Real max(Real a, Real b)
	{
		return (a > b) ? a : b;
	}

	class COREBASE_EXPORT MatrixCom
	{
	public:
		static float clampUnity( float x );
		static void transpose( osg::Matrix& dest, const osg::Matrix& src );
		static osg::Vec3 getRow3( const osg::Matrix& matrix, int row );
		static osg::Vec4 getRow4( const osg::Matrix& matrix, int row );
		static void setRow( osg::Matrix& matrix, const osg::Vec3& vec, int row );
		static void setRow( osg::Matrix& matrix, const osg::Vec4& vec, int row );
		static void hprToMatrix( osg::Matrix& rotation, const osg::Vec3& hpr );
		static void positionAndHprToMatrix( osg::Matrix& rotation, const osg::Vec3& xyz, const osg::Vec3& hpr );
		static void matrixToHpr( osg::Vec3& hpr, const osg::Matrix& rotation );
		static void matrixToHprAndPosition( osg::Vec3& xyz, osg::Vec3& hpr, const osg::Matrix& rotation );
		static void transformVec3(osg::Vec3& xyz, const osg::Matrix& transformMat);
		static void transformVec3(osg::Vec3& vec_in, const osg::Vec3& xyz, const osg::Matrix& transformMat);

		static float decompose(const osg::Matrix& M, osg::Matrix& Q, osg::Matrix& S, osg::Vec3& T);
		static void matCopyMinusEqual(osg::Matrix& C, const osg::Matrix& A);
		static void matBinOpEqualPlus(osg::Matrix& C, const float g1, const osg::Matrix& A, const float g2, const osg::Matrix& B);
		static void adjointTranspose(const osg::Matrix& M, osg::Matrix& MadjT);
		static float normInf(const osg::Matrix& M);
		static float normOne(const osg::Matrix& M);
		static int findMaxCol(const osg::Matrix& M);
		static void makeReflector(const osg::Vec3& v, osg::Vec3& u);
		static void reflectCols(osg::Matrix& M, const osg::Vec3& u);
		static void reflectRows(osg::Matrix& M, const osg::Vec3& u);
		static float matNorm(const osg::Matrix& M, const int tpose);
		static void doRank1(osg::Matrix& M, osg::Matrix& Q);
		static void doRank2(osg::Matrix& M, const osg::Matrix& MadjT, osg::Matrix& Q);

		static bool equivalent(float float1, float float2, float baseEpsilon = FLT_EPSILON)
		{
			return (abs(float1 - float2) <= baseEpsilon * max(1.0f, max(float1, float2)));
		}
	};
}